#include <iostream>
#include <stdio.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <vector>
#include "Vector/vector.03.h"

#define SQR(x) ((x)*(x))


using namespace std;


double G = 43007.1 ; // aprox

template <typename T1, typename T2>
Vector<double> get_a(vector< T1 > &p, Vector<T2> R,  double eps2) {
	
	Vector<double> a;
	
	for (uint i=0; i<p.size(); i++) {

		Vector<double> DR = p[i].R-R;
		
		double r2 = SQR(DR.x) + SQR(DR.y) + SQR(DR.z) + eps2;
		double r = sqrt(r2);
		double fac = p[i].m/(r2*r);

		a-= fac*DR;
		
	}
	//a.print();
	return G*a;
}

template <typename TP, typename TC, typename TW> 
double getSurfDen(vector<TP> &p, double r, double dr, Vector<TC> C, Vector<TW> W){

	double pi = 3.14159265358979323846;

	double r1 = r-dr > 0 ? r-dr : 0;
	double r2 = r+dr;

	double M(0);
	
	for (uint i=0; i<p.size(); i++) {
		
		Vector<double> Ri = p[i].R - C;
		double ri = sqrt(Ri.sqr()-SQR(Ri*W)); 
		
		if (r1<ri && ri< r2 ) M+=p[i].m;
		
	}
			
	double ds = pi*(r2*r2 - r1*r1);
			
	return M/ds;

}

template <typename T>
double getEpiKappa2(vector<T> &p, double r, double dr, double z, double eps2) {

	if (r<=0) return 0;
	
	double a1 = (get_a(p, Vector<double>(r,0,0), eps2)).x;
	double a2 = (get_a(p, Vector<double>(r+dr,0,0), eps2)).x;

	return 3 * a1 / r + (a2 - a1) / dr;
   
}


template <typename TD, typename TV, typename TM> 
TV findCM(vector<TD> &d, TV *vp, TM *mp){
	
	int ofs =  sizeof(d[0]);

	TM M(0);
	TV V; V*=0;
	
	for (uint i=0; i<d.size(); i++) {
		
		V += (*vp)*(*mp);
		M += *mp;
		
		vp = (TV*)((char*)vp+ofs);
		mp = (TM*)((char*)mp+ofs);
	
	}
	
	V/=M;
	
	return V;
}

struct VR{
	double v, m;
};

template <typename T> 
double getVelDispR(vector<T> &p, double r, double dr, dVector CR, dVector CV, dVector W){
			
	double r1 = r-dr>0 ? r-dr : 0;
	double r2 = r+dr;
	
	vector<VR> vr;

	for (uint i=0; i<p.size(); i++) {
		Vector<double> Ri = p[i].R-CR;
		double ri = sqrt(Ri.sqr()-SQR(Ri*W)); 
		
		if ((r1<ri && ri< r2 )/* &&  (-0.1 < p[i].R.z && p[i].R.z < +0.1)*/ ){
			VR _vr;
			_vr.v = (p[i].V-CV)*((Ri%W)%W).ort();
			_vr.m = p[i].m;
			vr.push_back(_vr);
		}
				
	}
	
	double vrcm= findCM(vr, &vr[0].v, &vr[0].m);
	
	for (uint i=0; i<vr.size(); i++)
		vr[i].v = pow(vr[i].v-vrcm, 2);
			
	return sqrt(findCM(vr, &vr[0].v, &vr[0].m));
};




template <typename T>
double getToomeQ(vector<T> &p, double r, double dr, double z, dVector Rcm, dVector Vcm, dVector W, double eps2) {
	
	return getVelDispR(p,r,dr,Rcm,Vcm,W)*sqrt( epi_kappa2(p,r,r+r*0.01,0, eps2) )/(3.36*G*getSurfDen(p,r,dr,Rcm,W));
}
